/*
 * Copyright (c) 1997, 1998, 2003, 2006 Aleksandar Samardzic
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. The name of the author may not be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <assert.h>		/* for assert() function */
#include <stdlib.h>		/* for malloc() function */
#include "polygon.h"
#include "edge.h"
#include "data_access_structure.h"
#include "voxel_grid.h"

/* Polygon_Init - Polygon class constructor */
void
Polygon_Init(this, numVertices)
     PPolygon       *this;
     DWord           numVertices;
{
	this->numVertices = numVertices;
	this->vertex = malloc(numVertices * sizeof(PVertex));
	assert(this->vertex != NULL);
}

/* Polygon_Clean - Polygon class destructor */
void
Polygon_Clean(this)
     PPolygon       *this;
{
	free(this->vertex);
}

/* Polygon_Intersect - intersect polygon with a line */
Bool
Polygon_Intersect(this, pLine)
     PPolygon       *this;
     PLine          *pLine;
{
	double         *p0,
	               *dp = pLine->dp;
	double          A = this->plane.A,
	    B = this->plane.B,
	    C = this->plane.C,
	    d = this->plane.d;
	double          t;
	PVector         crossPoint;
	int             ax,
	                ay;
	DWord           numVertices = this->numVertices;
	PVertex        *pVertex0,
	               *pVertex1,
	               *pEndVertex;
	double         *point0,
	               *point1;
	Bool            odd;
	double          den;

	/* intersect line with polygon plane */
	den = A * dp[X] + B * dp[Y] + C * dp[Z];

	if (den == 0)		/* line parallel to polygon plane */
		return FALSE;

	/* check if t is in range [EPSILON,pLine->t-EPSILON) */
	p0 = pLine->p0;
	if ((t = -(A * p0[X] + B * p0[Y] + C * p0[Z] - d) / den) < EPSILON
	    || t >= pLine->t - EPSILON)
		return FALSE;

	/* calculate point where line crosses polygon plane */
	Vector_LERP(crossPoint, p0, dp, t);

	/* point in polygon test */
	ax = this->plane.ax;
	ay = this->plane.ay;

	/* trace line from crossing point parallel to ax axis */
	odd = FALSE;		/* initially, parity is even (0
				 * intersections of line with polygon
				 * edges */
	pVertex0 = this->vertex;
	pVertex1 = this->vertex + numVertices - 1;
	pEndVertex = pVertex1 + 1;
	for (; pVertex0 < pEndVertex; pVertex1 = pVertex0++) {	/* for
								 * each
								 * vertex */
		/* if line crosses polygon edge, flip parity */
		point0 = pVertex0->point;
		point1 = pVertex1->point;
		if (((point0[ay] <= crossPoint[ay]
		      && crossPoint[ay] < point1[ay])
		     || (point1[ay] <= crossPoint[ay]
			 && crossPoint[ay] < point0[ay]))
		    && (crossPoint[ax] <
			point0[ax] + (point1[ax] -
				      point0[ax]) * (crossPoint[ay] -
						     point0[ay]) /
			(point1[ay] - point0[ay])))

			odd ^= TRUE;
	}

	if (!odd)		/* point is outside of polygon (even
				 * number of intersections) */
		return FALSE;

	pLine->t = t;
	return TRUE;
}

/* Polygon_CalcNormal - calculate polygon normal at specified point */
void
Polygon_CalcNormal(this, point, normal)
     PPolygon       *this;
     PVector         point;
     PVector         normal;
{
	DWord           vertex,
	                numVertices = this->numVertices;
	PVertex        *pVertex = this->vertex;
	DWord           index[3];
	double          distance[3];
	PVector         currPoint;
	double          norm2;
	PVertex        *pVertex0,
	               *pVertex1,
	               *pVertex2;
	Byte            ax,
	                ay;
	double          alpha,
	                beta;
	double          u0,
	                u1,
	                u2;
	double          v0,
	                v1,
	                v2;
	double          mul,
	                den;

	/* find indices of three vertices closest to point */
	index[0] = 0, index[1] = 1, index[2] = 2;
	distance[0] = distance[1] = distance[2] = DBL_MAX;
	for (vertex = 0; vertex < numVertices; vertex++, pVertex++) {
		Vector_Sub(currPoint, point, pVertex->point);
		if ((norm2 = Vector_DotProduct(currPoint, currPoint)) == 0) {	/* vertex 
										 * is 
										 * hit 
										 */
			memcpy(normal, pVertex->normal, sizeof(PVector));
			return;
		} else if (norm2 < distance[0]) {	/* current vertex
							 * is first
							 * closest to
							 * point so far */
			index[2] = index[1], distance[2] = distance[1];
			index[1] = index[0], distance[1] = distance[0];
			index[0] = vertex, distance[0] = norm2;
		} else if (norm2 < distance[1]) {	/* current vertex
							 * is second
							 * closest to
							 * point so far */
			index[2] = index[1], distance[2] = distance[1];
			index[1] = vertex, distance[1] = norm2;
		} else if (norm2 < distance[2])	/* current vertex is third 
						 * closest to point so far */
			index[2] = vertex, distance[2] = norm2;
	}

	/* store pointers to closest vertices */
	pVertex0 = this->vertex + index[0];
	pVertex1 = this->vertex + index[1];
	pVertex2 = this->vertex + index[2];

	/* determine barycentric coordinates alpha and beta of point */
	ax = this->plane.ax, ay = this->plane.ay;
	u0 = point[ax] - pVertex0->point[ax];
	u1 = pVertex1->point[ax] - pVertex0->point[ax];
	u2 = pVertex2->point[ax] - pVertex0->point[ax];
	v0 = point[ay] - pVertex0->point[ay];
	v1 = pVertex1->point[ay] - pVertex0->point[ay];
	v2 = pVertex2->point[ay] - pVertex0->point[ay];

	mul = 1 / (u1 * v2 - u2 * v1);
	alpha = (u0 * v2 - u2 * v0) * mul;
	beta = (u1 * v0 - u0 * v1) * mul;
	mul = 1 - (alpha + beta);

	/* interpolate normal */
	normal[X] =
	    mul * pVertex0->normal[X] + alpha * pVertex1->normal[X] +
	    beta * pVertex2->normal[X];
	normal[Y] =
	    mul * pVertex0->normal[Y] + alpha * pVertex1->normal[Y] +
	    beta * pVertex2->normal[Y];
	normal[Z] =
	    mul * pVertex0->normal[Z] + alpha * pVertex1->normal[Z] +
	    beta * pVertex2->normal[Z];

	Vector_Normalize(normal, mul, den);
}

/* Polygon_CalcBoundingVolume - calculate bounding volume for polygon */
void
Polygon_CalcBoundingVolume(this, pNode, pSlabs)
     PPolygon       *this;
     PBoundingVolume *pNode;
     PSlabs         *pSlabs;
{
	Byte            slab,
	                numSlabs = pSlabs->numSlabs;
	double          (*d)[2] = pNode->d;
	DWord           vertex,
	                numVertices = this->numVertices;
	PVertex        *pVertex = this->vertex;
	double          dCurr;

	/* initialize bounding volume */
	for (slab = 0; slab < numSlabs; slab++) {
		d[slab][LO] = DBL_MAX;
		d[slab][HI] = -DBL_MAX;
	}

	for (vertex = 0; vertex < numVertices; vertex++, pVertex++)	/* for 
									 * each 
									 * vertex */
		for (slab = 0; slab < numSlabs; slab++) {	/* for
								 * each
								 * slab */
			/* calculate current value of d */
			dCurr =
			    Vector_DotProduct(pVertex->point,
					      pSlabs->pVectors[slab]);

			/* adjust bounding volume if necessary */
			if (dCurr < pNode->d[slab][LO])
				pNode->d[slab][LO] = dCurr;
			if (dCurr > pNode->d[slab][HI])
				pNode->d[slab][HI] = dCurr;
		}
}

/* Polygon_Voxelize - voxelization algorithm for polygon according to
 * [Kauf87] */
void
Polygon_Voxelize(this, pGrid)
     PPolygon       *this;
     PVoxelGrid     *pGrid;
{
	DWord           numVertices = this->numVertices;
	double          (*d)[2] = pGrid->d;
	double          hCell = pGrid->hCell,
	    _hCell = 1 / hCell;
	double          A,
	                B,
	                C;
	Byte            u,
	                v,
	                w;
	PVertex        *pVertex0,
	               *pVertex1,
	               *pEndVertex;
	double         *v0,
	               *v1;
	PVector         p0,
	                p;
	DWord           voxel[2],
	                voxel0,
	                voxel1,
	                voxel_curr;
	double          t,
	                mul;
	DWord          *numETEdges,
	                numAETEdges,
	                gap;
	PEdge          *pEdge,
	              **ppEdge;
	PEdge        ***ET,
	              **ppETEdge,
	              **ppEndETEdge;
	PEdge         **AET,
	              **AETPrev,
	              **ppAETEdge,
	              **ppEndAETEdge,
	               *pAETEdge;
	double          v_curr;
	Bool            sorted;

	A = fabs(this->plane.A), B = fabs(this->plane.B), C =
	    fabs(this->plane.C);

	if (A <= B)
		if (A <= C) {
			u = X;
			if (B <= C)
				v = Y, w = Z;
			else
				v = Z, w = Y;
		} else
			u = Z, v = X, w = Y;
	else if (B <= C) {
		u = Y;
		if (A <= C)
			v = X, w = Z;
		else
			v = Z, w = X;
	} else
		u = Z, v = Y, w = X;

	ET = calloc(pGrid->numCells[v], sizeof(PEdge **));
        assert(ET != NULL);
	numETEdges = calloc(pGrid->numCells[v], sizeof(DWord));
        assert(numETEdges != NULL);
	voxel[LO] = pGrid->numCells[v], voxel[HI] = 0;
	for (pVertex1 = this->vertex, pEndVertex =
	     pVertex1 + numVertices, pVertex0 = pEndVertex - 1;
	     pVertex1 < pEndVertex; pVertex0 = pVertex1, pVertex1++) {
		pEdge = malloc(sizeof(PEdge));
		assert(pEdge != NULL);
		if (pVertex0->point[v] < pVertex1->point[v])
			v0 = pVertex0->point, v1 = pVertex1->point;
		else
			v0 = pVertex1->point, v1 = pVertex0->point;

		Vector_Assign(p0, v0);
		Vector_Sub(p, v1, v0);
		t = 1;
		VoxelGrid_EnumerateCellsLine(pGrid, p0, p, &t,
					     VoxelGrid_Insert, this);

		voxel0 = (DWord) ((v0[v] - d[v][LO]) * _hCell);
		voxel1 = (DWord) ((v1[v] - d[v][LO]) * _hCell);
		if (voxel0 != voxel1) {
			Vector_Assign(p0, v0);
			Vector_Sub(p, v1, v0);
			mul = 1 / p[v];
			t = (d[v][LO] + (voxel0 + 1) * hCell -
			     v0[v]) * mul;
			Vector_LERP(pEdge->point, p0, p, t);
			mul *= hCell;
			Vector_Mul(pEdge->delta, p, mul);
			pEdge->hi = voxel1;

			if (voxel0 < voxel[LO])
				voxel[LO] = voxel0;
			if (voxel1 > voxel[HI])
				voxel[HI] = voxel1;

			ET[voxel0] =
			    (PEdge **) realloc(ET[voxel0],
					       (++(numETEdges[voxel0])) *
					       sizeof(PEdge **));
			assert(ET[voxel0] != NULL);
			ppETEdge = ET[voxel0], ppEndETEdge =
			    ppETEdge + numETEdges[voxel0] - 1;
			for (; ppETEdge < ppEndETEdge; ppETEdge++)
				if ((*ppETEdge)->point[v] >
				    pEdge->point[v])
					break;
			memcpy(ppETEdge + 1, ppETEdge,
			       (ppEndETEdge - ppETEdge) * sizeof(PEdge *));
			*ppETEdge = pEdge;
		} else
			free(pEdge);
	}

	AET = NULL, numAETEdges = 0;
	for (voxel_curr = voxel[LO] + 1; voxel_curr <= voxel[HI];
	     voxel_curr++) {
		v_curr = d[v][LO] + voxel_curr * hCell;

		ppETEdge = ET[voxel_curr - 1], ppEndETEdge =
		    ppETEdge + numETEdges[voxel_curr - 1];
		AETPrev = AET, ppAETEdge = AETPrev, ppEndAETEdge =
		    ppAETEdge + numAETEdges;
		numAETEdges += numETEdges[voxel_curr - 1];
		AET = malloc(numAETEdges * sizeof(PEdge *));
		assert(AET != NULL);
		for (ppEdge = AET;
		     ppAETEdge < ppEndAETEdge && ppETEdge < ppEndETEdge;
		     ppEdge++)
			if ((*ppAETEdge)->point[v] < (*ppETEdge)->point[v])
				*ppEdge = *(ppAETEdge++);
			else
				*ppEdge = *(ppETEdge++);
		for (; ppAETEdge < ppEndAETEdge; ppEdge++)
			*ppEdge = *(ppAETEdge++);
		for (; ppETEdge < ppEndETEdge; ppEdge++)
			*ppEdge = *(ppETEdge++);
		free(ET[voxel_curr - 1]);
		free(AETPrev);

		for (ppAETEdge = AET, ppEndAETEdge = AET + numAETEdges;
		     ppAETEdge < ppEndAETEdge; ppAETEdge += 2) {
			if ((*ppAETEdge)->point[u] <
			    (*(ppAETEdge + 1))->point[u])
				v0 = (*ppAETEdge)->point, v1 =
				    (*(ppAETEdge + 1))->point;
			else
				v0 = (*(ppAETEdge + 1))->point, v1 =
				    (*ppAETEdge)->point;

			Vector_Assign(p0, v0);
			Vector_Sub(p, v1, v0);
			t = 1;
			p0[v] -= hCell / 2;
			VoxelGrid_EnumerateCellsLine(pGrid, p0, p, &t,
						     VoxelGrid_Insert,
						     this);
			p0[v] += hCell;
			VoxelGrid_EnumerateCellsLine(pGrid, p0, p, &t,
						     VoxelGrid_Insert,
						     this);
			p0[v] -= hCell / 2;
		}

		for (gap = 0, ppAETEdge = AET, ppEndAETEdge =
		     AET + numAETEdges; ppAETEdge < ppEndAETEdge;
		     ppAETEdge++)
			if ((pAETEdge = *ppAETEdge)->hi == voxel_curr) {
				numAETEdges--;
				free(pAETEdge);
				gap++;
			} else {
				Vector_AddAssign(pAETEdge->point,
						 pAETEdge->delta)
				    * (ppAETEdge - gap) = *ppAETEdge;
			}
		if (AET = realloc(AET, numAETEdges * sizeof(PEdge *)))
			FOREVER {
			sorted = TRUE;
			for (ppAETEdge = AET, ppEndAETEdge =
			     AET + numAETEdges - 1;
			     ppAETEdge < ppEndAETEdge; ppAETEdge++)
				if ((*ppAETEdge)->point[v] >
				    (*(ppAETEdge + 1))->point[v]) {
					sorted = FALSE;
					pEdge = *ppAETEdge;
					*ppAETEdge = *(ppAETEdge + 1);
					*(ppAETEdge + 1) = pEdge;
				}
			if (sorted)
				break;
			}
	}

	free(AET);
	free(numETEdges);
	free(ET);
}
